//-----------------------------------------------------------------
// Includes
//-----------------------------------------------------------------
#include "Processor.hpp"
#include "../Vision/VisionTools.hpp"
#include "../Tools/Logger.hpp"
#include <sstream>

//#define DEBUG_GRAPHIC 1 

namespace Calibration
{	
	///////////////////////////////////////////////////////////////////
	// Constructeur
	///////////////////////////////////////////////////////////////////
	CalibrationPositionSet::CalibrationPositionSet()
	{
	}
	
	///////////////////////////////////////////////////////////////////
	// Destructeur
	///////////////////////////////////////////////////////////////////
	CalibrationPositionSet::~CalibrationPositionSet()
	{
	}
		
	///////////////////////////////////////////////////////////////////
	// 
	///////////////////////////////////////////////////////////////////
	void CalibrationPositionSet::AddPosition( float* translationMatrix, float* rotationMatrix, int dataIndex )
	{
		Position position( translationMatrix, rotationMatrix );
		positionMap[ dataIndex ] = position;
	}

	///////////////////////////////////////////////////////////////////
	// 
	///////////////////////////////////////////////////////////////////
	CalibrationPositionSet::Position* CalibrationPositionSet::GetPosition( int dataIndex )
	{
		PositionMap::iterator iterator; 
		iterator = positionMap.find( dataIndex );
		return iterator!=positionMap.end()?&(iterator->second):NULL;
	}
}
